from machine import SoftI2C, Pin
from ssd1306 import SSD1306_I2C
import mpu6050
import time

oled_width = 128
oled_height = 64

scl = Pin('PC6')
sda = Pin('PC7')

def draw_bg():
    oled.fill(0)
    oled.line(0, 0, oled_width-1, 0, 1)
    oled.line(0, 0, 0, oled_height-1, 1)
    oled.line(oled_width-1, 0, oled_width-1, oled_height-1, 1)
    oled.line(0, oled_height-1, oled_width-1, oled_height-1, 1)

i2c = SoftI2C(scl, sda)
print("I2C Devices: %s" % ",".join(["%s" % i for i in i2c.scan()]))

oled = SSD1306_I2C(oled_width, oled_height, i2c)
draw_bg()
oled.text('MPU6050',40,20)
oled.show()
time.sleep(3)

for i in range(0,100):
    accelerometer = mpu6050.accel(i2c)
    vals = accelerometer.get_values()
    draw_bg()
    acx = vals["AcX"]
    acy = vals["AcY"]
    acz = vals["AcZ"]
    tmp = vals["Tmp"]
    gyx = vals["GyX"]
    gyy = vals["GyY"]
    gyz = vals["GyZ"]
    print(vals)

    # 0-127
    # 63-127
    draw_bg()
    oled.text('MPU6050',40,5)
    oled.text('AX:%03d' % acx,2,20)
    oled.text('AY:%03d' % acy,2,30)
    oled.text('AZ:%03d' % acz,2,40)

    oled.text('GX:%03d' % gyx,70,20)
    oled.text('GY:%03d' % gyy,70,30)
    oled.text('GZ:%03d' % gyz,70,40)
    
    oled.text('Temp:%02.1f' % tmp,25,55)    
    oled.show()

    time.sleep(1)


